/*
该任务用来处理来自不同控制方式下传回的期望的控制指令在此处统一为通用格式的控制指令发送给其他任务

*/



/* Includes ----------------------------------------------------------------- */
#include "cmd.h"
#include "gpio.h"

#include <string.h>

/* Private function  -------------------------------------------------------- */








/*Export function --------------------------------------------------------------*/
int8_t CMD_Init(CMD_t *cmd){
   /*若主结构体为空 自动返回错误 */
  if(cmd == NULL) return-1;
 /**/

  cmd->C_cmd.type =RC;
	cmd->C_cmd.mode =IMU;
	

return 0;
}


int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc){
   
   if (cmd == NULL) return -1;
   if (rc == NULL) return -1;

  /*将遥控器对应的原始指令改为通用的控制命令*/

     cmd->Vw = rc->ch_w * rc->mul;
     cmd->Vx = rc->ch_x * rc->mul;
	   cmd->Vy = -(rc->ch_y * rc->mul);

  //预留其他按键
   
return 0;
}

int32_t MD;//为什么是全局变量MD
void R12ds_ErrorDect(CMD_t *cmd,CMD_RC_t *rc)
{ 	
	if(cmd->C_cmd.mode!=RELAXED)
	{
	  MD=(cmd->C_cmd.mode);
	}
  if(rc->key[6]==300)
	{
		cmd->C_cmd.mode =RELAXED;
		HAL_GPIO_WritePin(GPIOH,LED_R_Pin,GPIO_PIN_SET);
	}	
   else 
   {
		cmd->C_cmd.mode =MD;
		 HAL_GPIO_WritePin(GPIOH,LED_R_Pin,GPIO_PIN_RESET);
	 }
}


int8_t CMD_ParseAct(CMD_t *cmd,CMD_ACTION_t *act){
  
  if(cmd == NULL) return -1;
  if(act == NULL) return -1;
  
	
	//是否在此处做一个量纲的转换 来取代底盘里的量纲转换
	
  cmd->Vx = act->out.Vx;
	cmd->Vw = act->out.Vw;
	cmd->Vy = act->out.Vy;
  

return 0;
}


int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
  if(cmd == NULL) return -1;
  if(n == NULL) return -1;

  cmd->cmd_status = n->status_fromnuc;
    switch(cmd->cmd_status){
			case PICK:
			cmd->C_pick.angle= n->pick.angle;
			cmd->C_pick.cmd= n->pick.cmd;
			cmd->C_pick.posy= n->pick.posy;
		  break;
			case NAVI:
				cmd->C_navi.vx = n->navi.vx;
			  cmd->C_navi.vy = n->navi.vy;
			  cmd->C_navi.wz = n->navi.wz;
			break;
			case PUT:
				cmd->C_put.angle = n->put.angle;
			  cmd->C_put.pos   = n->put.pos;
			break;
		}
		
  
return 0;
}


int8_t CMD_CtrlSet(CMD_t *cmd) {

 if(cmd == NULL) return -1;

 switch(cmd->cmd_status){
	 case PICK:
		 cmd->C_cmd.type = CAMERA_PICK;
	   break;
	 case PUT: 
		 cmd->C_cmd.type = CAMERA_PUT;
	   break;
	 case NAVI:
		 cmd->C_cmd.type = SLAM_NAVI;
		 
 }


return 0;
}



